#include "stdio.h"
#include "string.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "math.h"
#include "driver/uart.h"
#include "esp_system.h"

#include "iot_servo.h"
#include "drivers/oled_driver/OLED.h"

// Servo pin definitions
#define ROUND_SERVO_PIN 1
#define ROW_SERVO_PIN 2
// OpenMV pin definitions
#define OPENMV_UART_NUM UART_NUM_2
#define OPENMV_UART_TX_PIN 7
#define OPENMV_UART_RX_PIN 8
int8_t uart_data[5] = {0};
int uart_length;
float round_angle = 135.0;
float row_angle = 135.0;
int dx = 0;
int dy = 0;
TaskHandle_t uart_task_handle = NULL;
/*
@brief Initialize all components
@note This function should be called before using any other components.
*/
void all_init(void)
{
    ESP_LOGI("init", "Round servo pin: %d, Row servo pin: %d", ROUND_SERVO_PIN, ROW_SERVO_PIN);
    servo_config_t servo_cfg_round = {
        .max_angle = 270,
        .min_width_us = 500,
        .max_width_us = 2500,
        .freq = 50,
        .timer_number = LEDC_TIMER_0,
        .channels = {
            .servo_pin = {
                ROUND_SERVO_PIN,
            },
            .ch = {
                LEDC_CHANNEL_0,
            },
        },
        .channel_number = 1,
    };
    servo_config_t servo_cfg_row = {
        .max_angle = 270,
        .min_width_us = 500,
        .max_width_us = 2500,
        .freq = 50,
        .timer_number = LEDC_TIMER_0,
        .channels = {
            .servo_pin = {
                ROW_SERVO_PIN,
            },
            .ch = {
                LEDC_CHANNEL_1,
            },
        },
        .channel_number = 2,
    };
    iot_servo_init(LEDC_LOW_SPEED_MODE, &servo_cfg_round);
    iot_servo_init(LEDC_LOW_SPEED_MODE, &servo_cfg_row);
    iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, 135);
    iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 1, 135);
    ESP_LOGI("init", "Servos initialized and set to 135 degrees.");

    // Initialize OpenMV UART
    ESP_LOGI("init", "Initializing OpenMV UART on port %d with TX pin %d and RX pin %d",
             OPENMV_UART_NUM, OPENMV_UART_TX_PIN, OPENMV_UART_RX_PIN);
    uart_driver_install(OPENMV_UART_NUM, 2048, 2048, 0, NULL, 0);
    uart_config_t openmv_config = {
        .baud_rate = 115200,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .rx_flow_ctrl_thresh = 122,
        .source_clk = UART_SCLK_DEFAULT,
    };
    uart_param_config(OPENMV_UART_NUM, &openmv_config);
    uart_set_pin(OPENMV_UART_NUM, OPENMV_UART_RX_PIN, OPENMV_UART_TX_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
    memset(uart_data, 0, sizeof(uart_data));
    ESP_LOGI("init", "OpenMV UART initialized.");

    // Initialize Laser GPIO
    ESP_LOGI("init", "Initializing Laser GPIO.");
    gpio_config_t laser_gpio_config = {
        .pin_bit_mask = (1ULL << 3),
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE,
    };
    gpio_config(&laser_gpio_config);
    gpio_set_level(3, 0);
    ESP_LOGI("init", "Laser GPIO initialized.");

    // Initialize button GPIO
    ESP_LOGI("init", "Initializing Button GPIO.");
    gpio_config_t button_gpio_config = {
        .pin_bit_mask = (1ULL << 11) | (1ULL << 12) | (1ULL << 10) | (1ULL << 13) | (1ULL << 9),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_ENABLE,
        .intr_type = GPIO_INTR_DISABLE,
    };
    gpio_config(&button_gpio_config);
    ESP_LOGI("init", "Button GPIO initialized.");
}

void uart_data_process_task(void *arg)
{
    memset(uart_data, 0, sizeof(uart_data));
    while (1)
    {
        size_t buffered_size;
        uart_get_buffered_data_len(OPENMV_UART_NUM, &buffered_size);
        if (buffered_size > 0)
        {
            int len = uart_read_bytes(OPENMV_UART_NUM, uart_data, sizeof(uart_data), 100 / portTICK_PERIOD_MS);
            if (len > 0)
            {
                if (len >= 4)
                {
                    dx = (int)uart_data[2];
                    dy = (int)uart_data[3];
                }
                uart_flush_input(OPENMV_UART_NUM);
                memset(uart_data, 0, sizeof(uart_data));
            }
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}

void servo_control_task_NORMAL(void *arg)
{
    TickType_t start_time = xTaskGetTickCount(); // 记录任务开始时间
    while (1)
    {
        vTaskDelay(pdMS_TO_TICKS(10));
        TickType_t current_time = xTaskGetTickCount();
        TickType_t elapsed_time = current_time - start_time;

        // 激光打开条件：任务运行超过1s后，dx dy小于3 或者 任务运行超过2秒
        if (elapsed_time >= pdMS_TO_TICKS(1000) && ((abs(dx) < 3 && abs(dy) < 3) || (elapsed_time >= pdMS_TO_TICKS(2000))))
        {
            gpio_set_level(3, 1);
            // 激光打开后删除uart任务并清除dx dy
            if (uart_task_handle != NULL)
            {
                vTaskDelete(uart_task_handle);
                uart_task_handle = NULL;
                dx = 0; // 清除dx
                dy = 0; // 清除dy
                ESP_LOGI("servo_control_task_NORMAL", "UART task deleted after laser activation, dx dy cleared.");
            }
        }
        if ((abs(dx) < 20 && abs(dx) > 3) || (abs(dy) < 20 && abs(dy) > 3))
        {
            round_angle -= dx * 0.008;
            iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, round_angle);
            row_angle -= dy * 0.001;
            iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 1, row_angle);
            vTaskDelay(pdMS_TO_TICKS(15));
        }
        if (abs(dx) >= 20 || abs(dy) >= 20)
        {
            round_angle -= dx * 0.01;
            iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, round_angle);
            row_angle -= dy * 0.008;
            iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 1, row_angle);
            vTaskDelay(pdMS_TO_TICKS(20));
        }
    }
}

void servo_control_task_FUCK(void *arg)
{
    int attach = 0;                              // 0 no 1 yes
    TickType_t start_time = xTaskGetTickCount(); // 记录任务开始时间
    while (1)
    {
        while (attach == 0)
        {
            // 检查扫描超时
            TickType_t current_time = xTaskGetTickCount();
            TickType_t elapsed_time = current_time - start_time;

            // 如果超过3.8秒仍未检测到目标，跳出扫描循环
            if (elapsed_time >= pdMS_TO_TICKS(3800))
            {
                ESP_LOGI("servo_control_task_FUCK", "Scan timeout after 3.8s, breaking scan loop.");
                attach = 1;
                break;
            }

            if (dx == 0 || dy == 0)
            {
                // 逐行扫描参数
                static int scan_direction = 1;     // 1: 从左到右, -1: 从右到左
                static float scan_step = 2.0;      // 水平扫描步长
                static float line_step = 3.0;      // 垂直行间距
                static int scan_state = 0;         // 0: 水平扫描, 1: 垂直移动
                static int vertical_direction = 1; // 1: 向上, -1: 向下

                // 定义扫描范围
                static float min_round_angle = 70.0;  // 最左边
                static float max_round_angle = 200.0; // 最右边
                static float min_row_angle = 120.0;   // 最下边
                static float max_row_angle = 150.0;   // 最上边

                if (scan_state == 0) // 水平扫描状态
                {
                    round_angle += scan_direction * scan_step;

                    // 检查是否到达水平边界
                    if (scan_direction == 1 && round_angle >= max_round_angle)
                    {
                        round_angle = max_round_angle;
                        scan_state = 1; // 切换到垂直移动状态
                    }
                    else if (scan_direction == -1 && round_angle <= min_round_angle)
                    {
                        round_angle = min_round_angle;
                        scan_state = 1; // 切换到垂直移动状态
                    }
                }
                else // 垂直移动状态
                {
                    row_angle += vertical_direction * line_step;

                    // 检查是否到达垂直边界
                    if (vertical_direction == 1 && row_angle >= max_row_angle)
                    {
                        row_angle = max_row_angle;
                        vertical_direction = -1; // 改变垂直方向
                    }
                    else if (vertical_direction == -1 && row_angle <= min_row_angle)
                    {
                        row_angle = min_row_angle;
                        vertical_direction = 1; // 改变垂直方向
                    }

                    // 垂直移动完成，切换到下一行的水平扫描
                    scan_direction = -scan_direction; // 反转水平扫描方向
                    scan_state = 0;                   // 切换回水平扫描状态
                }

                iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, round_angle);
                iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 1, row_angle);
                vTaskDelay(pdMS_TO_TICKS(30)); // 调整扫描速度
            }
            else
            {
                attach = 1;
                break;
            }
        }
        while (attach == 1)
        {
            vTaskDelay(pdMS_TO_TICKS(10));
            TickType_t current_time = xTaskGetTickCount();
            TickType_t elapsed_time = current_time - start_time;
            // 激光打开条件：dx dy小于3 或者 任务运行超过4秒
            if ((abs(dx) < 3 && abs(dy) < 3) || (elapsed_time >= pdMS_TO_TICKS(4000)))
            {
                gpio_set_level(3, 1);
                // 激光打开后删除uart任务并清除dx dy
                if (uart_task_handle != NULL)
                {
                    vTaskDelete(uart_task_handle);
                    uart_task_handle = NULL;
                    dx = 0; // 清除dx
                    dy = 0; // 清除dy
                    ESP_LOGI("servo_control_task_FUCK", "UART task deleted after laser activation, dx dy cleared.");
                }
            }
            else
            {
                gpio_set_level(3, 0); // 确保激光关闭，当目标不在精确范围内且未超时时
            }
            if ((abs(dx) < 20 && abs(dx) > 3) || (abs(dy) < 20 && abs(dy) > 3))
            {
                round_angle -= dx * 0.008;
                iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, round_angle);
                row_angle -= dy * 0.001;
                iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 1, row_angle);
                vTaskDelay(pdMS_TO_TICKS(15));
            }
            if (abs(dx) >= 20 || abs(dy) >= 20)
            {
                round_angle -= dx * 0.01;
                iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, round_angle);
                row_angle -= dy * 0.008;
                iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 1, row_angle);
                vTaskDelay(pdMS_TO_TICKS(20));
            }
        }
    }
}

void oled_display_task(void *arg)
{
    char display_buffer[32];
    OLED_Init();
    while (1)
    {
        OLED_Clear();
        snprintf(display_buffer, sizeof(display_buffer), "%d", uart_data[1]);
        OLED_ShowString(0, 16, display_buffer, 6);
        snprintf(display_buffer, sizeof(display_buffer), "%d", uart_data[2]);
        OLED_ShowString(40, 16, display_buffer, 6);
        snprintf(display_buffer, sizeof(display_buffer), "%d", uart_data[3]);
        OLED_ShowString(80, 16, display_buffer, 6);
        OLED_ShowNum(0, 32, round_angle, 3, 8);
        OLED_ShowNum(50, 32, row_angle, 3, 8);
        OLED_Update();
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}

void button_process_task(void *arg)
{
    while (1)
    {
        if (gpio_get_level(11) == 1)
        {
            iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, 135);
            iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 1, 135);
            ESP_LOGI("button_process_task", "Button 1 pressed, resetting servos to 135 degrees.");
        }
        if (gpio_get_level(12) == 1)
        {
            xTaskCreatePinnedToCore(servo_control_task_NORMAL, "servo_control_task_NORMAL", 4096, NULL, 5, NULL, tskNO_AFFINITY);
            ESP_LOGI("button_process_task", "Button 2 pressed, starting normal servo control task.");
        }
        if (gpio_get_level(10) == 1)
        {
            xTaskCreatePinnedToCore(servo_control_task_FUCK, "servo_control_task_FUCK", 4096, NULL, 5, NULL, tskNO_AFFINITY);
            ESP_LOGI("button_process_task", "Button 3 pressed, starting fuck servo control task.");
        }
        if (gpio_get_level(13) == 1)
        {
            esp_restart();
        }
        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

void app_main(void)
{
    all_init();
    memset(uart_data, 0, sizeof(uart_data));
    ESP_LOGI("app_main", "Initialization complete. Starting tasks.");
    xTaskCreatePinnedToCore(oled_display_task, "oled_display_task", 4096, NULL, 4, NULL, tskNO_AFFINITY);
    xTaskCreatePinnedToCore(uart_data_process_task, "uart_data_process_task", 4096, NULL, 3, &uart_task_handle, tskNO_AFFINITY);
    xTaskCreatePinnedToCore(button_process_task, "button_process_task", 4096, NULL, 2, NULL, tskNO_AFFINITY);
}
